#ifndef __BOX2D__
#define __BOX2D__

#include <chrono>
#include <cmath>
#include <tuple>
#include <vector>

#include "line2d.h"
#include "vec2d.h"

namespace v2 {

inline auto PolygonArea(std::vector<Vec2D> crosses) -> double {
    int    n    = crosses.size();
    double area = 0;
    for (int i = 0; i < n; ++i) {
        area += crosses[i].x * crosses[(i + 1) % n].y;
        area -= crosses[(i + 1) % n].x * crosses[i].y;
    }
    area = abs(area) / 2.0; // 取绝对值并除以2得到多边形面积
    return area;
}

inline auto frand() -> double {
    using namespace std::chrono;
    std::srand(duration_cast<microseconds>(system_clock::now().time_since_epoch()).count());
    auto result = (rand() / double{RAND_MAX} - 0.5) * 2;
    return result;
};

class Circle;

class Box2D {
  public:
    Point  back_center_, real_center_;
    double length_2_front_, length_2_rear_, width_2_side_;
    Point  vertex_fl_, vertex_fr_, vertex_rl_, vertex_rr_;

  public:
    Box2D() = default;
    Box2D(double x, double y, double t, double l1, double l2, double sw)
            : back_center_(x, y, t), length_2_front_(l1), length_2_rear_(l2), width_2_side_(sw) {
        ResetBoxInfo();
    }

    void ResetBoxInfo() {
        auto cost = cos(back_center_.theta());
        auto sint = sin(back_center_.theta());

        auto move_length   = (length_2_front_ + length_2_rear_) / 2 - length_2_rear_;
        auto real_center_x = back_center_.x + cost * move_length;
        auto real_center_y = back_center_.y + sint * move_length;
        real_center_       = Point{real_center_x, real_center_y, back_center_.theta()};

        length_2_front_cost = length_2_front_ * cost;
        length_2_front_sint = length_2_front_ * sint;
        length_2_rear_cost = length_2_rear_ * cost;
        length_2_rear_sint = length_2_rear_ * sint;
        width_2_side_sint =  width_2_side_ * sint;
        width_2_side_cost = width_2_side_ * cost;

        vertex_fl_ = Point{back_center_.x + length_2_front_cost - width_2_side_sint,
                           back_center_.y + length_2_front_sint + width_2_side_cost};
        vertex_fr_ = Point{back_center_.x + length_2_front_cost + width_2_side_sint,
                           back_center_.y + length_2_front_sint - width_2_side_cost};
        vertex_rr_ = Point{back_center_.x - length_2_rear_cost + width_2_side_sint,
                           back_center_.y - length_2_rear_sint - width_2_side_cost};
        vertex_rl_ = Point{back_center_.x - length_2_rear_cost - width_2_side_sint,
                           back_center_.y - length_2_rear_sint + width_2_side_cost};
    }

    auto Length2Front() -> double { return length_2_front_; }
    auto Length2Rear() -> double { return length_2_rear_; }
    auto Width2Side() -> double { return width_2_side_; }
    auto BackCenter() -> Point { return this->back_center_; }
    auto RealCenter() -> Point { return real_center_; }
    auto FL() -> Point { return vertex_fl_; }
    auto FR() -> Point { return vertex_fr_; }
    auto RL() -> Point { return vertex_rl_; }
    auto RR() -> Point { return vertex_rr_; }
    auto Vertexs() -> std::vector<Point> { return std::vector<Point>{vertex_fl_, vertex_rl_, vertex_rr_, vertex_fr_}; }

    /* 判断矩形与点是否重叠 | 从园区物流项目和植树机器人项目中扒代码 */
    template<typename _PointType>
    auto OverlapWith(const _PointType& point) -> bool {
        auto rec_points = Vertexs();
        bool inside        = true;
        for (int i = 0; i < 4; i++) {
            Vec2D v1 = {rec_points[i].x - point.x, rec_points[i].y - point.y};                     // 计算向量v1(x1-x, y1-y)
            Vec2D v2 = {rec_points[(i + 1) % 4].x - point.x, rec_points[(i + 1) % 4].y - point.y}; // 计算向量v2(x2-x, y2-y)

            // 点到边的距离计算公式
            float distance = abs(v1.x * v2.y - v2.x * v1.y) / sqrt((v1.x * v1.x + v1.y * v1.y) * (v2.x * v2.x + v2.y * v2.y));

            // 如果所有点到该点的距离都小于该点到两个对角线的距离，则说明该点在矩形内
            Vec2D dia1 = {rec_points[(i + 2) % 4].x - point.x, rec_points[(i + 2) % 4].y - point.y};
            Vec2D dia2 = {rec_points[i].x - rec_points[(i + 2) % 4].x, rec_points[i].y - rec_points[(i + 2) % 4].y};
            float diag_distance =
                abs(dia1.x * dia2.y - dia2.x * dia1.y) / sqrt((dia1.x * dia1.x + dia1.y * dia1.y) * (dia2.x * dia2.x + dia2.y * dia2.y));
            if (distance <= diag_distance)
                inside = true; 
            else {
                inside = false;
                break;
            }
        }
        return inside;
    }

    /* 判断两矩形是否重叠 | 已验证 */
    auto OverlapWith(Box2D& other) -> bool {
        // 不考虑朝向 作一次初筛，如果
        auto max_length       = sqrt(pow(length_2_front_, 2) + pow(width_2_side_, 2));
        auto max_length_other = sqrt(pow(other.length_2_front_, 2) + pow(other.width_2_side_, 2));
        auto back_center_dis  = sqrt(pow(other.back_center_.x - back_center_.x, 2) + pow(other.back_center_.y - back_center_.y, 2));
        if (back_center_dis > max_length + max_length_other) return false;

        /* OBB */
        // 根据角点得到与两个矩形的两组垂直边平行的两组垂直轴
        std::vector<double> axis = {atan2(FL().y - FR().y, FL().x - FR().x),                         //
                                    atan2(FL().y - RL().y, FL().x - RL().x),                         //
                                    atan2(other.FL().y - other.FR().y, other.FL().x - other.FR().x), //
                                    atan2(other.FL().y - other.RL().y, other.FL().x - other.RL().x)};

        for (auto a : axis) {
            // 表示轴方向的向量
            Vec2D axis_direct(cos(a), sin(a));

            // 遍历本矩形的角点向量并计算出角点向量在a轴上的最大投影和最小投影
            double proj_max   = -MAX_FLOAT;
            double proj_min   = MAX_FLOAT;
            auto   vertex_vec = [](std::vector<Point> vertexs) -> std::vector<Vec2D> {
                std::vector<Vec2D> vec;
                for (const auto& vertex : vertexs) vec.emplace_back(Vec2D(vertex.x, vertex.y));
                return vec;
            };
            for (auto vertex_vec : vertex_vec(Vertexs())) {
                double temp_project = vertex_vec * axis_direct;
                proj_max            = std::max(proj_max, temp_project);
                proj_min            = std::min(proj_min, temp_project);
            }

            // 遍历另一个矩形的角点向量并计算出角点向量在a轴上的最大投影和最小投影
            double other_proj_max = -std::numeric_limits<double>::infinity();
            double other_proj_min = std::numeric_limits<double>::infinity();
            for (auto vertex_vec : vertex_vec(other.Vertexs())) {
                double temp_project = vertex_vec * axis_direct;
                other_proj_max      = std::max(other_proj_max, temp_project);
                other_proj_min      = std::min(other_proj_min, temp_project);
            }

            // 如果投影没有重合则返回没有重合
            if (other_proj_max - proj_min < 0 or proj_max - other_proj_min < 0) return false;
        }
        return true;
    }

    /* wait for requirements */
    auto OverlapWith(const Circle& other) -> bool;

    /* shake */
    auto ShakePos(double intensity) -> Box2D {
        return Box2D(back_center_.x + frand() * intensity, back_center_.y + frand() * intensity, back_center_.theta(), length_2_front_,
                     length_2_rear_, width_2_side_);
    }

    auto ShakeHeading(double intensity) -> Box2D {
        return Box2D(back_center_.x, back_center_.y, back_center_.theta() + frand() * intensity, length_2_front_, length_2_rear_,
                     width_2_side_);
    }

    auto ShakeShape(double intensity) -> Box2D {
        return Box2D(back_center_.x, back_center_.y, back_center_.theta(),
                     length_2_front_ + frand() * intensity * length_2_front_ / (length_2_front_ + length_2_rear_),
                     length_2_rear_ + frand() * intensity * length_2_rear_ / (length_2_front_ + length_2_rear_),
                     width_2_side_ + frand() * intensity / 2);
    }

    auto ShakeByParams(std::tuple<double, double, double, double, double, double> params, std::tuple<double, double, double> intensities)
        -> Box2D {
        auto [a, b, c, d, e, f]                                  = params;
        auto [pos_intensity, heading_intensity, shape_intensity] = intensities;
        return Box2D(back_center_.x + a * pos_intensity, back_center_.y + b * pos_intensity, back_center_.theta() + c * heading_intensity,
                     length_2_front_ + d * shape_intensity * length_2_front_ / (length_2_front_ + length_2_rear_),
                     length_2_rear_ + e * shape_intensity * length_2_rear_ / (length_2_front_ + length_2_rear_),
                     width_2_side_ + f * shape_intensity / 2);
    }

}; // class Box2D

} // namespace v2

#endif // __BOX2D__